#include <netinet/in.h>
#include <netdb.h>
#include <stdlib.h>
#include <stdio.h>
#include <string.h>
#include <sys/types.h>
#include <sys/socket.h>
#include <unistd.h>
#include <localization/test/localization_test.h>

namespace {
bool connectToOutput();
const std::vector<player_point_2d> MappedLamps();

int sockfd;
}

void TESTLocalization() {
  using namespace PlayerCc;

  bool connected = connectToOutput();

  player_pose2d initial_mean = { -16.0, -12.0, 0.0 };
  gsl_matrix *initial_covariance = gsl_matrix_alloc(3, 3);
  gsl_matrix_set_zero(initial_covariance);
  gsl_matrix_set(initial_covariance, 0, 0, 0.01);
  gsl_matrix_set(initial_covariance, 1, 1, 0.01);
  gsl_matrix_set(initial_covariance, 2, 2, 0.0001);

  PlayerClient player_client("localhost", 6665);
  Position2dProxy position2d_proxy(&player_client, 0);
  SimulationProxy simulation_proxy(&player_client, 0);

  position2d_proxy.RequestGeom();
  position2d_proxy.ResetOdometry();
  position2d_proxy.SetMotorEnable(1);
  player_client.Read();
  player_client.Read();

  SyncedLampCentroids lamp_centroids;
  SyncedCurrentPose current_pose;
  SyncedCurrentCovariance current_covariance;

  LocalizationEstimator localization_estimator(&initial_mean,
          initial_covariance, MappedLamps(), &player_client, &position2d_proxy,
          &current_pose, &current_covariance, &lamp_centroids);
  FakeLampDetector lamp_detector(MappedLamps(), &simulation_proxy, &lamp_centroids);

  boost::thread localization_estimator_thread(
          boost::ref(localization_estimator));
  boost::thread lamp_detector_thread(boost::ref(lamp_detector));

  char robot_name[] = "robot";
  char outputBuffer[100];

  gsl_matrix *covariance = gsl_matrix_alloc(3, 3);

  while (true) {
    player_pose2d estimated_pose = current_pose.current_pose();
    current_covariance.current_covariance(covariance);

    double sim_x, sim_y, sim_yaw;
    simulation_proxy.GetPose2d(robot_name, sim_x, sim_y, sim_yaw);

    sprintf(outputBuffer, "%9.5lf %9.5lf %9.5lf %9.5lf %9.5lf %9.5lf %9.5lf %9.5lf %9.5lf\n",
            sim_x, sim_y, sim_yaw, estimated_pose.px, estimated_pose.py, estimated_pose.pa,
            gsl_matrix_get(covariance, 0, 0), gsl_matrix_get(covariance, 1, 1),
            gsl_matrix_get(covariance, 0, 1));

    if (connected && write(sockfd, outputBuffer, strlen(outputBuffer)) < 0)
      connected = false;

    sleep(1);
  }

  gsl_matrix_free(covariance);
}

namespace {
bool connectToOutput() {
  int port = 60500;
  struct sockaddr_in serv_addr;
  struct hostent *server;

  sockfd = socket(AF_INET, SOCK_STREAM, 0);
  server = gethostbyname("localhost");
  if (sockfd < 0 || server == NULL) {
    printf("WARNING: Could not connect to output!\n\n");
    return false;
  }

  bzero((char *) &serv_addr, sizeof(serv_addr));
  serv_addr.sin_family = AF_INET;
  bcopy((char *) server->h_addr, (char *) &serv_addr.sin_addr.s_addr, server->h_length);
  serv_addr.sin_port = htons(port);

  if (connect(sockfd, (struct sockaddr *) &serv_addr, sizeof(serv_addr)) < 0) {
    printf("WARNING: Could not connect to output!\n\n");
    return false;
  }
  return true;
}

const std::vector<player_point_2d> MappedLamps() {
  std::vector<player_point_2d> mapped_lamps;

  player_point_2d mapped_lamp;

  // Corridor
  for (int i = -16; i <= 16; i += 4) {
    mapped_lamp.px = i;
    mapped_lamp.py = 0;
    mapped_lamps.push_back(mapped_lamp);
  }

  // Left Rooms
  mapped_lamp.px = -14;
  mapped_lamp.py = 8;
  mapped_lamps.push_back(mapped_lamp);

  mapped_lamp.py = -8;
  mapped_lamps.push_back(mapped_lamp);

  // Center Rooms
  mapped_lamp.px = 0;
  mapped_lamp.py = 8;
  mapped_lamps.push_back(mapped_lamp);

  mapped_lamp.py = -8;
  mapped_lamps.push_back(mapped_lamp);

  // Right Rooms
  mapped_lamp.px = 12;
  mapped_lamp.py = 8;
  mapped_lamps.push_back(mapped_lamp);

  mapped_lamp.py = -8;
  mapped_lamps.push_back(mapped_lamp);

  return mapped_lamps;
}
}
